//
// Created by fl on 23-9-11.
//

#include "zmq_client.h"

ZmqClient::ZmqClient(std::string ue4_ip, uint32_t ue4_port, uint32_t interval_ms)
        : interval_us_(interval_ms * 1000)
{
    std::cout << "ZMQ_client start, " << " msg from " << ue4_ip
    << ": " << (int)ue4_port << std::endl;

    context_ = zmq_ctx_new();
    std::string send_str = "tcp://" + ue4_ip + ":" + std::to_string(ue4_port);

    socket_ = zmq_socket(context_, ZMQ_REQ);
    zmq_connect(socket_, send_str.c_str());
}

ZmqClient::~ZmqClient() {
    zmq_close(socket_);
    zmq_ctx_destroy(context_);
}

int ZmqClient::request_num(std::string request_str) {
    std::vector<std::string> request_num_str = {"Target", "tar", "Vehicle", "veh", "Obstacle", "obs"};

    if (std::find(request_num_str.begin(), request_num_str.end(), request_str) != request_num_str.end()){
        zmq_msg_t reques_msg;
        zmq_msg_init_size(&reques_msg, request_str.size());
        memcpy(zmq_msg_data(&reques_msg), request_str.c_str(), request_str.size());
                zmq_msg_send(&reques_msg, socket_, 0);
        zmq_msg_close(&reques_msg);

        zmq_msg_t reply_message;
        zmq_msg_init(&reply_message);
        zmq_msg_recv(&reply_message, socket_, 0);

        std::string msg_str(static_cast<char*>(zmq_msg_data(&reply_message)), zmq_msg_size(&reply_message));
        int num =  std::stoi(msg_str);
        return num;
    }
    else {
        std::cout << "the passed string parameter is no in the optional range !\n";
        return -1;
    }

}


void ZmqClient::request_msg(std::string request_str, bool req_once) {
    
    std::unique_lock<std::mutex> lock(udpdata_mtx_, std::defer_lock);
    target_name_str_ = request_str;

    while (true) {
        uint64_t time_us_loop_start = currentTimeUs();
        
        zmq_msg_t reques_msg;
        zmq_msg_init_size(&reques_msg, target_name_str_.size());
        memcpy(zmq_msg_data(&reques_msg), target_name_str_.c_str(), target_name_str_.size());
        zmq_msg_send(&reques_msg, socket_, 0);
        zmq_msg_close(&reques_msg);

        zmq_msg_t reply_message;
        zmq_msg_init(&reply_message);
        zmq_msg_recv(&reply_message, socket_, 0);

        std::string msg_str(static_cast<char*>(zmq_msg_data(&reply_message)), zmq_msg_size(&reply_message));

        if (zmq_msg_size(&reply_message) > 10) {
//            std::cout << "request and recv spend " << elapsedTimeMs(time_loop_start) << " ms" << std::endl;

            KeyValueMap kv_map;
            string_to_map(msg_str, kv_map);

//            std::cout << "translate " << elapsedTimeMs(time_loop_start) << " ms" << std::endl;
            uint64_t time_now = currentTimeMs();

            kv_map["time_stamp_ms"] = time_now;

            lock.lock();
            target_info_map_ = kv_map;
            lock.unlock();
            updated_ = true;
            if(req_once == true){break;}
        } else {
            std::cout << "ZMQ_client recv msg error, msg size is " <<  (int)zmq_msg_size(&reply_message) << std::endl;
        }

        zmq_msg_close(&reply_message);

//        std::cout << "loop end " << elapsedTimeMs(time_loop_start) << "ms" << std::endl;

        int loop_elapsed_time_us = elapsedTimeUs(time_us_loop_start);

        if (loop_elapsed_time_us < interval_us_) {
            usleep(interval_us_ - loop_elapsed_time_us);
//            std::cout << "loop spent " << (int)loop_elapsed_time_us;
//            std::cout << "us, sleep " << (int)(interval_us_ - loop_elapsed_time_us)
//            << "us" << std::endl;
        } else {
//            std::cout << "zmq reques interval set to " << (int)interval_us_ << "us, "
//            << "but spent " << (int)elapsedTimeUs(time_us_loop_start) << std::endl;
        }
    }

}

int ZmqClient::string_to_map(const std::string &data, KeyValueMap &kv_map) {
    std::regex pattern("Key:(\\w+)_Value:(-*\\d+\\.?\\d*);");
    std::smatch match;

    // Find and store key-value pairs
    std::string::const_iterator searchStart(data.cbegin());
    while (std::regex_search(searchStart, data.cend(), match, pattern)) {
        if (match.size() == 3) {
            double value = std::stod(match[2].str());
            kv_map[match[1].str()] = value;
        }
        searchStart = match.suffix().first;
    }

    // Check for any missing or invalid key-value pairs
    if (kv_map.empty()) {
        std::cout << "No valid key-value pairs found." << std::endl;
        return 0;
    }
    return 1;
}

bool ZmqClient::updated(TargetInfo &target) {
    if (updated_) {
        updated_ = false;
        KeyValueMap target_map = get_target_map();
        target.index = (uint8_t)target_map["id"];
        target.time_stamp_ms = (uint64_t)target_map["time_stamp_ms"];
        target.lat = target_map["lat"] * 1e-7;
        target.lon = target_map["lon"] * 1e-7;
        target.alt = (float)target_map["alt"];
        target.spd_n = (float)target_map["vx"];
        target.spd_e = (float)target_map["vy"];
        target.spd_d = (float)target_map["vz"];
        return true;
    }
    return false;
}

KeyValueMap ZmqClient::get_target_map() {
    KeyValueMap data;
    std::unique_lock<std::mutex> lock(udpdata_mtx_);
    data = target_info_map_;
    lock.unlock();
    return data;
}
